#!/usr/bin/python3
# -*- coding: utf-8 -*-

import time
import EventLog as log
import numpy as np

import D_R4_LinearMotor_20230922 as D_R4_LinearMotor
import D_R4_RotaryMotor_20230920 as D_R4_RotaryMotor
# import D_record_jointstate as record
# import D_Liner2Joint2 as transformer
import D_Liner2Joint2 as D_Liner2Joint_hand
import D_Liner2Joint as D_Liner2Joint_leg
# import D_R4_IMU_V1

RAD2DEG = 57.2957786666
# ! 丝杆电机的方向偏转不在这里设置，是直接写在程序中的
JointOpposite = [11,22,31,32,33,35]

JointIDList = [11,12,13,14,15,16
               ,21,22,23,24,25,26
              ,31, 32, 33, 34, 35,36,37
              ,41, 42, 43, 44, 45,46,47
               ,71,72]


class LinkJointCmd:
    def __init__(self):
        # 实例化各种电机驱动
        #self.Record = record.Record_State()

        self.RMotor = D_R4_RotaryMotor.RotaryMotorNet()
        self.LMotor = D_R4_LinearMotor.LinearMotorNet()
        self.L2J_EJ = D_Liner2Joint_hand.ElbowJoint()
        self.L2J_WJ = D_Liner2Joint_hand.WristJoint()
        self.L2J_TJ = D_Liner2Joint_leg.ThighJoint()
        self.L2J_KJ = D_Liner2Joint_leg.KneeJoint()
        self.L2J_AJ = D_Liner2Joint_leg.AnkleJoint()
        
    def SetJointData(self,JointsNameList, JointsSetDataList):
        JointsSendDataList = {}
        for i, jointid in enumerate(JointsNameList):
            if jointid in [11,12,21,22,71,72,31, 32, 33, 35,41, 42, 43, 45]: # 腿部旋转电机,直接输入角度
                if jointid in JointOpposite:
                    JointsSendDataList[jointid] = -JointsSetDataList[i]
                else:
                    JointsSendDataList[jointid] = JointsSetDataList[i]
                if jointid == 22:JointsSendDataList[jointid] = JointsSendDataList[jointid]
            elif jointid in [13,23]:
                JointsSendDataList[jointid] = self.L2J_TJ.angle2lenght(JointsSetDataList[i])
            elif jointid in [14,24]:
                JointsSendDataList[jointid] = self.L2J_KJ.angle2lenght(JointsSetDataList[i])
            elif jointid in [34,44]:
                JointsSendDataList[jointid] = self.L2J_EJ.angle2lenght(JointsSetDataList[i])
            elif jointid == 15: # !必须有16
                index16 = JointsNameList.index(16)
                JointsSendDataList[15], JointsSendDataList[16] = self.L2J_AJ.angle2lenght(JointsSetDataList[i],-JointsSetDataList[index16])
            elif jointid == 25:# !必须有26
                index26 = JointsNameList.index(26)
                JointsSendDataList[25],JointsSendDataList[26] = self.L2J_AJ.angle2lenght(JointsSetDataList[i],-JointsSetDataList[index26])
            elif jointid == 36: # !必须有37
                index37 = JointsNameList.index(37)
                JointsSendDataList[36], JointsSendDataList[37] = self.L2J_WJ.angle2lenght(JointsSetDataList[i],-JointsSetDataList[index37])
                #print("set:j36,j37,l36,l37=",JointsSetDataList[i],JointsSetDataList[index37],JointsSendDataList[36], JointsSendDataList[37])
            elif jointid == 46:# !必须有47
                index47 = JointsNameList.index(47)
                JointsSendDataList[46],JointsSendDataList[47] = self.L2J_WJ.angle2lenght(JointsSetDataList[i],JointsSetDataList[index47])
                #print("set:j46,j47,l46,l47=",JointsSetDataList[i],JointsSetDataList[index47],JointsSendDataList[46], JointsSendDataList[47])
            elif jointid in [16,26]:
                pass
                # ! 在jointid==15,25时已经计算并执行了
            elif jointid in [37,47]:
                pass
                # ! 在jointid==37,47时已经计算并执行了
            else:
                pass

        for jointid in JointsSendDataList:
            #record_name = 'J'+str(jointid)
            if jointid in [11,12,21,22,71,72,31, 32, 33, 35,41, 42, 43, 45]:
                # 直线电机最大速度15000，加速度200
                self.RMotor.SetMotorData(jointid,JointsSendDataList[jointid],7000,200)
                # 记录
                #self.Record.publisher(record_name,JointsSendDataList[jointid])
            elif jointid in [13,14,23,24,34,44,36,37,46,47]:
                # 直线电机最大速度20000，加速度200
                self.LMotor.SetMotorData(jointid,JointsSendDataList[jointid],10000, 200)
                #self.Record.publisher(record_name,JointsSendDataList[jointid])
                #print("JointsSendDataList=",JointsSendDataList[jointid])
            elif jointid in [15,16,25,26]:
                # 直线电机最大速度20000，加速度200
                self.LMotor.SetMotorData(jointid,JointsSendDataList[jointid],15000, 200)
                #self.Record.publisher(record_name,JointsSendDataList[jointid])
                # print("JointsSendDataList",JointsSendDataList)
            else:
                #self.LMotor.SetMotorData(jointid, JointsSendDataList[jointid], 5000, 50)
                pass
            
    def GetJointData(self, JointsNameList):
        ret = []
        rlt = []

        for i, jointid in enumerate(JointsNameList):
            if jointid in [11,12,21,22,71,72,31,32,33,35,41,42,43,45]:
                r = self.RMotor.GetMotorData(jointid)
                ret.append(r[0])
                if jointid in JointOpposite:
                    rlt.append(-r[1])
                else:
                    rlt.append(r[1])
            elif jointid in [13,23]:
                r = self.LMotor.GetMotorData(jointid)
                ret.append(r[0])
                r1 = self.L2J_TJ.lenght2angle(r[1])
                rlt.append(round(r1,2))
            elif jointid in [14,24]:
                r = self.LMotor.GetMotorData(jointid)
                ret.append(r[0])
                r1 = self.L2J_KJ.lenght2angle(r[1])
                rlt.append(round(r1,2))
            elif jointid in [15,25]:
                r5 = self.LMotor.GetMotorData(jointid)
                r6 = self.LMotor.GetMotorData(jointid+1) # !读16or26电机
                # print("L15,L16",jointid,r5,r6)
                ret.append(min(r5[0],r6[0]))
                r = self.L2J_AJ.lenght2angle(r5[1],r6[1])
                rlt.append(round(r[0],2))
            elif jointid in [16,26]:
                r5 = self.LMotor.GetMotorData(jointid-1) # !读15or25电机
                r6 = self.LMotor.GetMotorData(jointid)
                ret.append(min(r5[0],r6[0]))
                r = self.L2J_AJ.lenght2angle(r5[1],r6[1])
                rlt.append(round(r[1],2))
            elif jointid in [34,44]:
                r = self.LMotor.GetMotorData(jointid)
                ret.append(r[0])
                r1 = self.L2J_EJ.lenght2angle(r[1])
                rlt.append(round(r1,2))
            elif jointid in [36,46]:
                r6 = self.LMotor.GetMotorData(jointid)
                r7 = self.LMotor.GetMotorData(jointid+1) # !读36or37电机
                #print("Get:jointid,r6,r7",jointid,r6,r7)
                ret.append(min(r6[0],r7[0]))
                r = self.L2J_WJ.lenght2angle(r6[1],r7[1])
                rlt.append(round(r[0],2))#36 返回值是r[0],37是返回值r[1]
                
            elif jointid in [37,47]:
                r6 = self.LMotor.GetMotorData(jointid-1)
                r7 = self.LMotor.GetMotorData(jointid) # !读46or47电机
                #print("Get:jointid,r6,r7",jointid,r6,r7)
                ret.append(min(r6[0],r7[0]))
                r = self.L2J_WJ.lenght2angle(r6[1],r7[1])
                if jointid == 47:
                    rlt.append(round(-r[1],2))
                else:
                    rlt.append(round(r[1],2))
   
                
            else:
                ret.append(0)
                rlt.append(0)

        return ret,rlt

    def __del__(self):
        pass
if __name__ == "__main__":

    L1 = LinkJointCmd()

    # ret,rlt = L1.GetJointData(JointIDList)
    # print(ret,rlt)
    # print("ret = ",dict(zip(JointIDList,ret)))
    # print("rlt = ",dict(zip(JointIDList,rlt)))
    # time.sleep(1)
    
    # JointsNameList = [31,32,33,34,35,36,37,41,42,43,44,45,46,47]
    # L1.SetJointData(JointsNameList, [10,10,10,10,10,0,10,10,10,10,10,10,0,18])
    # time.sleep(1)

    # JointList = [31,32,36,37,41,42,46,47]
    # L1.SetJointData(JointList,[20,20,0,10,20,20,-30,0])
    # # time.sleep(2)
    # m = L1.GetJointData(JointList)
    # print(m)

    JointList = [36,37,46,47]
    L1.SetJointData(JointList,[-20,-12,40,-2])
    time.sleep(1)
    m = L1.GetJointData(JointList)
    print(m)
    L1.SetJointData(JointList,[0,0,0,0])
    time.sleep(1)
    # m = L1.GetJointData(JointList)
    # print(m)
    
    # print(L1.GetJointData(JointList))
    # time.sleep(1)


    del L1

